﻿using System;
using System.Collections.Generic;
using System.Text;
using Geometry;

namespace PotentialFieldDriver.robotrace.drivers.implementation
{
    public class SimpleGoToTheNextPointDriver : RobotDriverInterface
    {
        private List<Point2D> optimalPath;
        private int currentCheckpoint = 0;
        private double maxVelocity = 1;
        // parameter stating how much speed can change in one step
        // given in fraction of the max speed
        private double velocityChangeInertiaParameter = 0.25;

        public SimpleGoToTheNextPointDriver(List<Point2D> optimalPath)
        {
            this.optimalPath = optimalPath;
        }

        public void step(FIRADriverLibrary.FIRARobotState robot)
        {
            if (optimalPath == null || optimalPath.Count <= 0)
            {
                return;
            }

            if (currentCheckpoint >= optimalPath.Count)
            {
                currentCheckpoint = 0;
            }

            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double currentRobotAngle = robot.angle;
            double currentRobotLeftSpeed = robot.leftSpeed;
            double currentRobotRightSpeed = robot.rightSpeed;

            double distanceFromNextCheckpoint = optimalPath[currentCheckpoint].GetDistance(currentRobotXPosition, currentRobotYPosition);

            while (robotReachedCheckpoint(distanceFromNextCheckpoint))
            {
                currentCheckpoint++;
                if (currentCheckpoint >= optimalPath.Count){
                    currentCheckpoint = 0;
                }
                distanceFromNextCheckpoint = optimalPath[currentCheckpoint].GetDistance(currentRobotXPosition, currentRobotYPosition);
            }

            Vector2D distanceVector = new Vector2D(new Point2D(currentRobotXPosition, currentRobotYPosition), optimalPath[currentCheckpoint]);
            Vector2D robotMoveVector = new Vector2D(currentRobotAngle) * (1 + (currentRobotLeftSpeed + currentRobotRightSpeed)/2);

            double differenceAngle = Vector2D.AngleBetween(robotMoveVector, distanceVector);
            Console.WriteLine(distanceVector + " ," + robotMoveVector + ", " + differenceAngle + ", left: " + currentRobotLeftSpeed + ", right: " + currentRobotRightSpeed);
            if (differenceAngle > -Math.PI && differenceAngle <= -Math.PI/2)
            {
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, - maxVelocity);
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, - maxVelocity * adjust(1 - (Math.PI + differenceAngle)));
            }
            else if (differenceAngle <= 0)
            {
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, maxVelocity);
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, maxVelocity * adjust(Math.PI / 2 + differenceAngle));
            }
            else if (differenceAngle <= Math.PI/2)
            {
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, maxVelocity);
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, maxVelocity* adjust(Math.PI / 2 - differenceAngle));
            }
            else
            {
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, - maxVelocity);
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, -maxVelocity * adjust(1 - (Math.PI - differenceAngle)));
            }

        }

        private bool robotReachedCheckpoint(double distanceFromNextCheckpoint)
        {
            if (distanceFromNextCheckpoint <= 0.05)
            {
                return true;
            }
            return false;
        }

        private Double adjust(Double d)
        {
            return Math.Abs(d/Math.PI*2);
        }

        private double changeSpeed(double previousSpeed, double destinationSpeed)
        {
            double difference = Math.Abs(previousSpeed - destinationSpeed);
            double maxSpeedChange = maxVelocity * velocityChangeInertiaParameter;
            if (difference < maxSpeedChange)
            {
                return destinationSpeed;
            }
            double sign = previousSpeed > destinationSpeed ? -1.0 : 1.0;
            return previousSpeed + sign * maxSpeedChange;
        }



        public System.Drawing.Bitmap getBitmap()
        {
            return null;
        }
    }
}
